#include "ugv_sdk/scout/scout_base.hpp"
#include <iostream>
#include <termios.h>  // 用于终端控制（实时读取键盘）
#include <unistd.h>   // 用于usleep

using namespace westonrobot;

// 终端设置：禁用行缓冲和回显，实现实时读取单个按键（无需按回车）
struct termios original_termios;
void disable_terminal_buffering() {
    tcgetattr(STDIN_FILENO, &original_termios);  // 保存原始终端设置
    struct termios new_termios = original_termios;
    new_termios.c_lflag &= ~(ICANON | ECHO);  // 禁用 canonical模式和回显
    new_termios.c_cc[VMIN] = 1;  // 至少读取1个字符
    new_termios.c_cc[VTIME] = 0; // 不超时
    tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
}

// 恢复终端原始设置
void restore_terminal() {
    tcsetattr(STDIN_FILENO, TCSANOW, &original_termios);
}

int main(int argc, char **argv) {
    // 初始化小车连接（CAN总线）
    ScoutBase scout;
    int32_t baud_rate = 0;
    scout.Connect("can0", baud_rate);
        
    scout.EnableCommandedMode();  // 启用指令模式
    std::cout << "小车连接成功！" << std::endl;
    std::cout << "控制说明：W=前进，S=后退，A=左转，D=右转，Q=退出" << std::endl;

    // 配置终端为实时按键模式
    disable_terminal_buffering();

    // 速度参数（根据小车性能调整，单位：线速度m/s，角速度rad/s）
    const double forward_speed = 1.0;    // 前进/后退速度
    const double rotate_speed = 1.0;     // 转向角速度

    bool running = true;
    while (running) {
        char key;
        read(STDIN_FILENO, &key, 1);  // 读取按键

        // 根据按键发送运动指令
        switch (key) {
            case 'w':
            case 'W':
                std::cout << "前进: 线速度=" << forward_speed << "m/s，角速度=0" << std::endl;
                scout.SetMotionCommand(forward_speed, 0.0);
                break;
            case 's':
            case 'S':
                std::cout << "后退: 线速度=" << -forward_speed << "m/s，角速度=0" << std::endl;
                scout.SetMotionCommand(-forward_speed, 0.0);
                break;
            case 'a':
            case 'A':
                std::cout << "左转: 线速度=0，角速度=" << rotate_speed << "rad/s" << std::endl;
                scout.SetMotionCommand(0.0, rotate_speed);
                break;
            case 'd':
            case 'D':
                std::cout << "右转: 线速度=0，角速度=" << -rotate_speed << "rad/s" << std::endl;
                scout.SetMotionCommand(0.0, -rotate_speed);
                break;
            case 'q':
            case 'Q':
                std::cout << "退出程序，停止小车..." << std::endl;
                running = false;
                break;
            default:
                // 按其他键停止小车
                std::cout << "停止: 线速度=0，角速度=0" << std::endl;
                scout.SetMotionCommand(0.0, 0.0);
                break;
        }

        usleep(50000);  // 50ms刷新一次（避免指令发送过于频繁）
    }

    // 退出前确保小车停止
    scout.SetMotionCommand(0.0, 0.0);
    // 恢复终端设置（否则终端可能无法正常输入）
    restore_terminal();
    // 断开连接
    scout.Disconnect();
    std::cout << "程序已退出" << std::endl;

    return 0;
}
